clear all
close all

syms m1 m2 u1 u2
syms c11 c12 c21 c22 k11 k12 k21 k22

m = [m1 m2];
u = [u1 u2];

C = [c11 c12; c21 c22];
K = [k11 k12; k21 k22];

% const_scale = 1/((2*pi)^(2/2))
% 
% denom = 1/(sqrt(det(C+K)))
% 
% numera = exp(-0.5*(m-u)*inv(C+K)*(m-u)')

x = [1 2];
mu = [4 3];
covar = [2 1; 1 3];
g11 = (gauss(mu, covar+[0.0001 0; 0 0], x)-gauss(mu, covar, x))/0.0001
g12 = (gauss(mu, covar+[0 0.0001; 0 0], x)-gauss(mu, covar, x))/0.0001
g21 = (gauss(mu, covar+[0 0; 0.0001 0], x)-gauss(mu, covar, x))/0.0001
g22 = (gauss(mu, covar+[0 0 ;0 0.0001], x)-gauss(mu, covar, x))/0.0001
grad1 = [g11 g12; g21 g22]

grad_mu =    -gauss(mu, covar, x)*inv(covar)*(x-mu)'
grad_covar =  gauss(mu, covar, x)*(0.5*inv(covar)*(x-mu)'*(x-mu)*inv(covar)-0.5*inv(covar))


% R = rand(2)
% C = rand(2)
% E = rand(2)
% mu = rand(2, 1)
% oldR = R;
% 
% delta = 0.01;
% for i = 1:3
%     delta = delta/10
% %     oo = (R*mu)'*inv(R*E*R'+C)*(R*mu);
% %     R = oldR + [delta 0; 0 0];
% %     o11 = (R*mu)'*inv(R*E*R'+C)*(R*mu);
% %     R = oldR + [0 delta; 0 0];
% %     o12 = (R*mu)'*inv(R*E*R'+C)*(R*mu);
% %     R = oldR + [0 0; delta 0];
% %     o21 = (R*mu)'*inv(R*E*R'+C)*(R*mu);
% %     R = oldR + [0 0; 0 delta];
% %     o22 = (R*mu)'*inv(R*E*R'+C)*(R*mu);
% %     (oo-[o11 o12; o21 o22])/delta
%     oo = mu'*R*E*R'*mu
%     R = oldR + [delta 0; 0 0];
%     o11 = mu'*R*E*R'*mu;
%     R = oldR + [0 delta; 0 0];
%     o12 = mu'*R*E*R'*mu;
%     R = oldR + [0 0; delta 0];
%     o21 = mu'*R*E*R'*mu;
%     R = oldR + [0 0; 0 delta];
%     o22 = mu'*R*E*R'*mu;
%     (oo-[o11 o12; o21 o22])/delta
%     
%     
%     inv(R*E*R')*mu*mu'*inv(R*E*R')
% end
% 
% syms a11 a12 a13 a21 a22 a23 a31 a32 a33
% syms b11 b12 b13 b21 b22 b23 b31 b32 b33
% 
% A = [a11 a12 a13; a21 a22 a23; a31 a32 a33];
% B = [b11 b12 b13; b21 b22 b23; b31 b32 b33];
% 
% C = A*B*A.'
% 
% 
% pretty(diff(C, a11))
% pretty(A*diff(B*A.', a11)+diff(A*B, a11)*A.')
% 
% diff(B*A.', a12)
% diff(A*B, a21)